<HTML> 
<HEAD> 
	<TITLE>3rd Party ROBOTC Drivers</TITLE> 
<link href="tabs.css" rel="stylesheet" type="text/css"/> 
<link href="doxygen.css" rel="stylesheet" type="text/css"/> 
</HEAD> 
 
<BODY bgcolor="#ffffff" link="#000000" vlink="#000000"> 
<table width="100%" bgcolor="navy" cellspacing=0 cellpadding=1 border=0> 
<tr><td><table width="100%" bgcolor="#EEEEDD" cellspacing=0 cellpadding=3 border=0> 
<tr> 
    <td width="33%" align="left"> <img src="images/LOGO_NXT.gif" width=266 height=44 border="0" alt="Mindstorms"></td> 
    <td><b><font size="+3" color="navy">3rd Party ROBOTC Drivers</font></b></td> 
    <td align="right"> <img src="images/logo.png" width=44 height=44 border="0" alt="RobotC"></td>     
</tr> 
  <tr bgcolor="#cccc99" align="right"> 
          <td colspan=3> <font face="arial" size="-1"> [<a href=main.html>Home</a>] [<a target=_top href="https://sourceforge.net/projects/rdpartyrobotcdr/">Download</a>] [<a target=_top href="http://apps.sourceforge.net/mantisbt/rdpartyrobotcdr/my_view_page.php">Submit a bug/suggestion</a>]  [<a target=_top href="http://www.robotc.net/forums/">ROBOTC Forums</a>] [<a target=_top href="http://botbench.com">Blog</a>] [<a target=_top href="http://sourceforge.net/donate/index.php?group_id=257238">Support this project</a>]</font> </td> 
</tr> 
</table></td></tr> 
</table> 
<!-- Generated by Doxygen 1.7.2 -->
<div class="header">
  <div class="headertitle">
<h1>mindsensors-ps2ctrl-v4-test1.c</h1>  </div>
</div>
<div class="contents">
<div class="fragment"><pre class="fragment"><span class="preprocessor">#pragma config(Sensor, S1,     PSPNXV4,        sensorI2CCustomFastSkipStates)</span>
<span class="preprocessor"></span><span class="preprocessor">#pragma config(Motor,  motorA,          MOT_A,     tmotorNormal, PIDControl, encoder)</span>
<span class="preprocessor"></span><span class="preprocessor">#pragma config(Motor,  motorB,          MOT_B,     tmotorNormal, PIDControl, encoder)</span>
<span class="preprocessor"></span><span class="preprocessor">#pragma config(Motor,  motorC,          MOT_C,     tmotorNormal, PIDControl, encoder)</span>
<span class="preprocessor"></span><span class="comment">//*!!Code automatically generated by &#39;ROBOTC&#39; configuration wizard               !!*//</span>

<span class="comment">/*</span>
<span class="comment"> * $Id: mindsensors-ps2ctrl-v4-test1.c 133 2013-03-10 15:15:38Z xander $</span>
<span class="comment"> */</span>
<span class="comment"></span>
<span class="comment">/**</span>
<span class="comment"> * mindsensors-ps2ctrl-v4.h provides an API for the Mindsensors PS2 controller sensor</span>
<span class="comment"> * with referee signal receiver.  This program demonstrates how to use that API</span>
<span class="comment"> * to control a 3-wheeled omniwheeled robot.</span>
<span class="comment"> *</span>
<span class="comment"> * Changelog:</span>
<span class="comment"> * - 0.1: Initial release</span>
<span class="comment"> *</span>
<span class="comment"> * Credits:</span>
<span class="comment"> * - Big thanks to Mindsensors for providing me with the hardware necessary to write and test this.</span>
<span class="comment"> *</span>
<span class="comment"> * License: You may use this code as you wish, provided you give credit where it&#39;s due.</span>
<span class="comment"> *</span>
<span class="comment"> * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER. </span>
<span class="comment"></span>
<span class="comment"> * Xander Soldaat (xander_at_botbench.com)</span>
<span class="comment"> * 02 August 2012</span>
<span class="comment"> * version 0.1</span>
<span class="comment"> */</span>


<span class="preprocessor">#include &quot;<a class="code" href="mindsensors-ps2ctrl-v4_8h.html" title="Playstation 2 Controller Sensor.">drivers/mindsensors-ps2ctrl-v4.h</a>&quot;</span>

<span class="preprocessor">#define MAXMOTORSPEED 90.0</span>
<span class="preprocessor"></span>
<span class="comment">// Use some trig to culcalate the required power for each motor</span>
<span class="keywordtype">void</span> MoveRobot(<span class="keywordtype">int</span> angle, <span class="keywordtype">int</span> Vb, <span class="keywordtype">int</span> rotSpeed) {
  <span class="keywordtype">float</span> Vw1, Vw2, Vw3, maxSpeed, norm_factor;
  <span class="keywordtype">int</span> iVw1, iVw2, iVw3;

  <span class="comment">// This is where the magic happens.  The actual formula is</span>
  <span class="comment">// Vw = rotSpeed + Vb * ((cosDegrees(wheelAngle) * cosDegrees(movementAngle)) + (sinDegrees(wheelAngle) * sinDegrees(movementAngle)))</span>
  <span class="comment">// I have optimised the formulae below to reduce the number of operations required to calculate the motor speeds</span>
  <span class="comment">// since the motorAngle value never changes and cancels some things out, further simplifying the calculations.</span>
  <span class="comment">// I like simple!</span>

  Vw1 = rotSpeed + Vb * cosDegrees(angle);
  Vw2 = rotSpeed + Vb * (-0.5 * cosDegrees(angle) + 0.866025 * sinDegrees(angle));
  Vw3 = rotSpeed + Vb * (-0.5 * cosDegrees(angle) - 0.866025 * sinDegrees(angle));

  <span class="comment">// This makes sure the motor values never exceed the maximum (MAXMOTORSPEED)</span>
  maxSpeed = <a name="a0"></a><a class="code" href="group__common.html#gaa0d45e1e67c5cacf5c9251fcd90a971b">max3</a>(abs(Vw1), abs(Vw2), abs(Vw3));

  norm_factor = (maxSpeed &gt; MAXMOTORSPEED) ? (MAXMOTORSPEED / maxSpeed) : 1.0;

  iVw1 = round(Vw1 * norm_factor);
  iVw2 = round(Vw2 * norm_factor);
  iVw3 = round(Vw3 * norm_factor);

  motor[motorA] = iVw1;
  motor[motorB] = iVw2;
  motor[motorC] = iVw3;
}


task main ()
{
  <span class="comment">// This is the struct that holds all the info on all buttons and joypads/sticks</span>
  <a name="_a1"></a><a class="code" href="structt_p_s_p.html">tPSP</a> controller;

  <span class="keywordtype">int</span> angle;
  <span class="keywordtype">int</span> speed;
  <span class="keywordtype">int</span> rotation;

  <span class="keywordflow">while</span> (<span class="keyword">true</span>)
  {
    <span class="comment">// Read the state of the buttons</span>
    <a name="a2"></a><a class="code" href="group___p_s_p4.html#gad139284e853e183891c69201568b465c">PSPV4readButtons</a>(PSPNXV4, controller);

    <span class="comment">// Two controls are used:</span>
    <span class="comment">// The left joystick controls speed and direction</span>
    <span class="comment">// The right joystick controls rotational speed</span>

    <span class="comment">// Calculate the angle we need to travel at using simple geometry</span>
    angle = radiansToDegrees(atan2(-controller.<a name="a3"></a><a class="code" href="structt_p_s_p.html#a4f0149c625b664e3ea802e50b16fb68f">joystickLeft_x</a>, -controller.<a name="a4"></a><a class="code" href="structt_p_s_p.html#adf4baea875d5bf6ce4394e5ac05cd8b5">joystickLeft_y</a>));

    <span class="comment">// This is the length of the vector, which is based on the amount we&#39;ve moved the</span>
    <span class="comment">// joystick in the X and Y directions</span>
    speed = sqrt((controller.<a class="code" href="structt_p_s_p.html#a4f0149c625b664e3ea802e50b16fb68f">joystickLeft_x</a> * controller.<a class="code" href="structt_p_s_p.html#a4f0149c625b664e3ea802e50b16fb68f">joystickLeft_x</a>) +
                                    (controller.<a class="code" href="structt_p_s_p.html#adf4baea875d5bf6ce4394e5ac05cd8b5">joystickLeft_y</a>*controller.<a class="code" href="structt_p_s_p.html#adf4baea875d5bf6ce4394e5ac05cd8b5">joystickLeft_y</a>));

                <span class="comment">// Rotation speed is controlled by the right joystick</span>
                rotation = -controller.<a name="a5"></a><a class="code" href="structt_p_s_p.html#af61c8a3bb96e7385b72f78fa99560ae4">joystickRight_x</a>;

    nxtDisplayCenteredTextLine(1, <span class="stringliteral">&quot;%d:%d&quot;</span>, controller.<a class="code" href="structt_p_s_p.html#a4f0149c625b664e3ea802e50b16fb68f">joystickLeft_x</a>, controller.<a class="code" href="structt_p_s_p.html#adf4baea875d5bf6ce4394e5ac05cd8b5">joystickLeft_y</a>);
    nxtDisplayCenteredTextLine(3, <span class="stringliteral">&quot;%d:%d&quot;</span>, controller.<a class="code" href="structt_p_s_p.html#af61c8a3bb96e7385b72f78fa99560ae4">joystickRight_x</a>, controller.<a name="a6"></a><a class="code" href="structt_p_s_p.html#a59602bcb0bfa6382a709f06f76a7aa8b">joystickRight_y</a>);
                nxtDisplayCenteredTextLine(5, <span class="stringliteral">&quot;%d&quot;</span>, angle);
                nxtDisplayCenteredTextLine(6, <span class="stringliteral">&quot;%d&quot;</span>, speed);
                nxtDisplayCenteredTextLine(7, <span class="stringliteral">&quot;%d&quot;</span>, rotation);
                MoveRobot(angle, speed, rotation);
    wait1Msec(100);
  }
  PlaySound(soundBeepBeep);
  MoveRobot(0, 0, 0);
  <span class="keywordflow">while</span> (bSoundActive) EndTimeSlice();
  wait1Msec(100);
}


<span class="comment">/*</span>
<span class="comment"> * $Id: mindsensors-ps2ctrl-v4-test1.c 133 2013-03-10 15:15:38Z xander $</span>
<span class="comment"> */</span>
</pre></div> </div>
</div>
<hr class="footer"/><address class="footer"><small>Generated on Wed Mar 13 2013 19:56:42 for ROBOTC Drivers by&#160;
<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.7.2 </small></address>
</body>
</html>
